// Ted Carancho's MikroQuadAero - September 2008
// An Arudino based quadrocopter using the Sparkfun
// 5DOF IMU and IDG300 Dual Axis Gyro
// This version will look at only gyro measurements
// to provide flight stability

#include <EEPROM.h>
#include <ServoTimer2.h>

// ******************** Initialize Variables ********************
#define BAUD 38400

// Define sensor pin assignments
#define ROLLRATEPIN 4
#define PITCHRATEPIN 3
#define YAWRATEPIN 5

// Define transmitter commands
#define TIMEOUT 2500
#define XMITFACTOR_ADR 12
#define THROTTLEPIN 4
#define ROLLPIN 2
#define PITCHPIN 3
#define YAWPIN 6
#define GEARPIN 7
#define AUXPIN 5
volatile int throttle = 0;
volatile int roll = 0;
volatile int pitch = 0;
volatile int yaw = 0;
volatile int gear = 0;
volatile int aux= 0;
int rollCommand = 1500;
int pitchCommand = 1500;
int yawCommand = 1500;
int rollCommandZero, pitchCommandZero, yawCommandZero;
// Controls the strength of the commands sent from the transmitter
// xmitFactor ranges from 0.01 - 1.0 (0.01 = weakest, 1.0 - strongest)
float xmitFactor;


// Define motor commands
#define MINCOMMAND 1000
#define MAXCOMMAND 2000
#define FRONTMOTORPIN 8
#define REARMOTORPIN 9
#define RIGHTMOTORPIN 10
#define LEFTMOTORPIN 11
ServoTimer2 frontMotor;
ServoTimer2 rearMotor;
ServoTimer2 rightMotor;
ServoTimer2 leftMotor;
int frontCommand;
int rearCommand;
int rightCommand;
int leftCommand;
int rollMotorCommand = 1500;
int pitchMotorCommand = 1500;
int yawMotorCommand = 1500;

// Gyro setup
// If AREF = 3.3V, then A/D is 931 at 3V and 465 = 1.5V
// Gyro range is from +/-500 deg/sec
#define ZEROLIMIT 5
int rollGyroZero, pitchGyroZero, yawGyroZero;
int rollRate, pitchRate, yawRate;

// Scale gyro rate (-465 to +465) to motor commands (1000 to 2000)
// use y = mx + b
// m = (y2 - y1) / (x2 - x1) = (2000 - 1000) / (465 - (-465))
float mMotorRate = 1.0753;
// b = y1 - m * x1
float bMotorRate = 1500;

// Smoothing parameters
int rollSmoothArray [16];
int pitchSmoothArray [16];
int yawSmoothArray [16];

// PID Values
#define WINDUP_GUARD_GAIN 100.0
#define PGAIN_ADR 0
#define IGAIN_ADR 4
#define DGAIN_ADR 8
float pTerm, iTerm, dTerm; 
float pgain = 0;
float igain = 0;
float dgain = 0;
float iRollState = 0;
float lastRollPosition = 0;
float iPitchState = 0;
float lastPitchPosition = 0;
float iYawState = 0;
float lastYawPosition = 0;

// Communication
volatile char queryType = 'X';
char string[32];
byte armed;

// Interrupt Handler
volatile byte timeSlot;

// Timing
long previousTime = 0;
long currentTime = 0;
long deltaTime = 0;

// ******************** Setup MikroQuadAero ********************
void setup() {
  Serial.begin(BAUD);
  analogReference(EXTERNAL); // Current external ref is connected to 3.3V
  pinMode(THROTTLEPIN, INPUT);
  pinMode(ROLLPIN, INPUT);
  pinMode(PITCHPIN, INPUT);
  pinMode(YAWPIN, INPUT);
  
  // Configure motors
  frontMotor.attach(FRONTMOTORPIN);
  rearMotor.attach(REARMOTORPIN);
  rightMotor.attach(RIGHTMOTORPIN);
  leftMotor.attach(LEFTMOTORPIN);
  commandAllMotors(MINCOMMAND);

  // Read PID values form EEPROM
  pgain = readFloat(PGAIN_ADR);
  igain = readFloat(IGAIN_ADR);
  dgain = readFloat(DGAIN_ADR);

  // Read transmit factor from EEPROM
  xmitFactor = readFloat(XMITFACTOR_ADR);

  // Setup interrupt to trigger on pin D2 (roll receiver pin)
  attachInterrupt(0, interruptHandler, RISING);
  armed = 0;
  
  // Wait for ESC's to initialize
  delay(2000);

  // Calibrate gyros
  zeroGyros();

  // Continue to wait for ESC's to initialize
  armESC(5000);

  previousTime = millis();
}

// ******************** Main MikroQuadAero Loop ********************
void loop () {
  // Measure loop rate
  currentTime = millis();
  deltaTime = currentTime - previousTime;
  previousTime = currentTime;
  
  // Send configuration commands form transmitter
  if (throttle < 1010) {
    // Disarm motors (throttle down, yaw left)
    if (yaw < (MINCOMMAND + 100) && armed == 1) {
      armed = 0;
      commandAllMotors(MINCOMMAND);
    }
    
    // Zero gyros (throttle down, pitch up)
    if (yaw < (MINCOMMAND + 100) && (pitch > (MAXCOMMAND - 100))) {
      zeroGyros();
    }
    
    // Arm motors (throttle down, yaw right)  
    if (yaw > (MAXCOMMAND - 100) && armed == 0) {
      armed = 1;
      commandAllMotors(MINCOMMAND + 20);
      delay(500);
      commandAllMotors(MINCOMMAND);
    }
  }

  // Read Sensors
  // Average sensor values and force them to be centered at 0
  rollRate = smooth(analogRead(ROLLRATEPIN), rollSmoothArray) - rollGyroZero;
  pitchRate = smooth(analogRead(PITCHRATEPIN), pitchSmoothArray) - pitchGyroZero;
  yawRate = smooth(analogRead(YAWRATEPIN), yawSmoothArray) - yawGyroZero;
  
  // Calculate Commanded Angles
  // Reduce transmitter commands using xmitFactor
  rollCommand = ((roll - 1500) * xmitFactor) + 1500;
  pitchCommand = ((pitch - 1500) * xmitFactor) + 1500;
  yawCommand = ((yaw - 1500) * xmitFactor) + 1500;
  
  // Update PID
  // Also convert sensor values to same units as PWM pulse length
  rollMotorCommand = updatePID((mMotorRate * rollRate) + bMotorRate, rollCommand, &lastRollPosition, &iRollState);
  pitchMotorCommand = updatePID((mMotorRate * pitchRate) + bMotorRate, pitchCommand, &lastPitchPosition, &iPitchState);
  yawMotorCommand = updatePID((mMotorRate * yawRate) + bMotorRate, yawCommand, &lastYawPosition, &iYawState);   

  // Calculate motor commands
  frontCommand = constrain((throttle - pitchMotorCommand + yawMotorCommand), MINCOMMAND, MAXCOMMAND);
  rearCommand = constrain((throttle + pitchMotorCommand + yawMotorCommand), MINCOMMAND, MAXCOMMAND);
  rightCommand = constrain(throttle - rollMotorCommand - yawMotorCommand, MINCOMMAND, MAXCOMMAND);
  leftCommand = constrain((throttle + rollMotorCommand - yawMotorCommand), MINCOMMAND, MAXCOMMAND);
    
  // Command motors
  if (armed) {
    frontMotor.write(frontCommand);
    rearMotor.write(rearCommand);
    rightMotor.write(rightCommand);
    leftMotor.write(leftCommand);
  }
  
  // Check for serial message
  if (Serial.available()) {
    queryType = Serial.read();
    if (queryType == 'U') {
      // Update PID values    
      pgain = readFloatSerial();
      writeFloat(pgain, PGAIN_ADR);
      igain = readFloatSerial();
      writeFloat(igain, IGAIN_ADR);
      dgain = readFloatSerial();
      writeFloat(dgain, DGAIN_ADR);
      // Update transmitter factor
      xmitFactor = readFloatSerial();
      writeFloat(xmitFactor, XMITFACTOR_ADR);
      queryType = 'X';
    }
  }
}

// ******************** Subroutines ********************
// Used to read floating point values from the serial port
float readFloatSerial() {
  int i = -1;
  char data[128] = "";
  
  do {
    while (Serial.available() == 0) delay(50); // wait if no data
    i++;
    data[i] = Serial.read();
  } while (data[i] != ';'); // read serial port until semi-colon found
  
  return strtod(data, NULL);
}

// Sends commands to all motors
void commandAllMotors(int motorCommand) {
  frontMotor.write(motorCommand);
  rearMotor.write(motorCommand);
  rightMotor.write(motorCommand);
  leftMotor.write(motorCommand);
}

// Removes noise from sensor data
int smooth(int rawIn, int *smooth_array) {
  long total = 0;
  static int i;

  i = (i + 1) % 16;
  smooth_array[i] = rawIn;
  for (int j = 0; j < 16; j++) total += smooth_array[j];
  return total >> 4 ;
}

// Used to average the sensors measurements when everything is level
int findZero(int channel) {
  int zero = 0;
  
  for (int i=0; i< 16; i++) zero += analogRead(channel);
  return zero >> 4;
}

// Allows user to zero gyros on command
void zeroGyros() {
  do {
    rollGyroZero = findZero(ROLLRATEPIN);
  } while (abs(analogRead(ROLLRATEPIN) - rollGyroZero) > ZEROLIMIT);
  
  do {
    pitchGyroZero = findZero(PITCHRATEPIN);
  } while (abs(analogRead(PITCHRATEPIN) - pitchGyroZero) > ZEROLIMIT);

  do {
    yawGyroZero = findZero(YAWRATEPIN);
  } while (abs(analogRead(YAWRATEPIN) - yawGyroZero) > ZEROLIMIT);
}

void armESC(unsigned long msWaitTime) {
  unsigned long startTime;
  startTime = millis();
  while ((millis() - startTime) < msWaitTime) {
    digitalWrite(FRONTMOTORPIN, HIGH);
    digitalWrite(REARMOTORPIN, HIGH);
    digitalWrite(RIGHTMOTORPIN, HIGH);
    digitalWrite(LEFTMOTORPIN, HIGH);
    delay(1);
    digitalWrite(FRONTMOTORPIN, LOW);
    digitalWrite(REARMOTORPIN, LOW);
    digitalWrite(RIGHTMOTORPIN, LOW);
    digitalWrite(LEFTMOTORPIN, LOW);
    delay(20);
  }
}
